package es.uji.viselab.math;

import javax.vecmath.Matrix4d;
import javax.vecmath.Vector3d;

public class Pose {

	protected Vector3d position;
	protected Orientation orientation;

	public Pose(Matrix4d m) {
		position = new Vector3d();
		position.x = m.m03;
		position.y = m.m13;
		position.z = m.m23;
		orientation = new Orientation();
	}

	public Pose(double[] xyzuvw) {
		position = new Vector3d();
		position.x = xyzuvw[0];
		position.y = xyzuvw[1];
		position.z = xyzuvw[2];
		orientation = new Orientation(xyzuvw[3], xyzuvw[4], xyzuvw[5]);
	}

	//public Pose(int x, int y, int z, int u, int v, int w) {
	//	this((double) x, (double) y, (double) z, (double) u, (double) v, (double) w);
	//}

	public Pose(double x, double y, double z, double u, double v, double w) {
		position = new Vector3d();
		position.x = x;
		position.y = y;
		position.z = z;
		orientation = new Orientation(u, v, w);
	}

	public double getX() {
		return position.x;
	}

	public double getY() {
		return position.y;
	}

	public double getZ() {
		return position.z;
	}

	public double getOrientationX() {
		return orientation.getX();
	}

	public double getOrientationY() {
		return orientation.getY();
	}

	public double getOrientationZ() {
		return orientation.getZ();
	}

	public Orientation getOrientation() {
		return orientation;
	}

}
